/*
 * boot.S
 *
 *  Created on: Nov 23, 2013
 *      Author: Nishanth
 */

.set SECTION_ENTRY_CTRL_BITS,	0xC02
.set SECTION_NEXT,		0x00100000
.set INIT_STACK_SIZE, 0x8000
.set INIT_PGTBL_SIZE, 0x4000
.set INIT_L2_PGTBL_SIZE, 0x400

/* mode bits in CPSR for different modes */

.set MODE_USR, 0x10
.set MODE_FIQ, 0x11
.set MODE_IRQ, 0x12
.set MODE_SVC, 0x13
.set MODE_ABT, 0x17
.set MODE_UND, 0x1B
.set MODE_SYS, 0x1F

.equ I_F_BIT, 0xC0

/* Stack sizes for different modes */

.set UND_STACK_SIZE,	0x400
.set ABT_STACK_SIZE,	0x400
.set FIQ_STACK_SIZE,	0x400
.set IRQ_STACK_SIZE,	0x1000
.set SVC_STACK_SIZE,	0x400

.global _stack
.global _bss_start
.global _bss_end
.global kernel_init

.global tlb_l1_base
.global tlb_l2_base

.equ UART_THR, 0x44E09000

.global __start
.global _und_Exc_hndlr
.global _SWI_Exc_hndlr
.global _abrtPrefetch_Exc_hndlr
.global _abrtData_Exc_hndlr
.global _reserved_Exc_hndlr
.global _IRQ_Exc_hndlr
.global _FIQ_Exc_hndlr

.section .start.text, "ax"

.global _start

_start:
/* Exception Handler */
	b __start					/* Reset handler */
	b _und_Exc_hndlr			/* Undefined handler */
	b _SWI_Exc_hndlr			/* Software INTR Handler */
	b _abrtPrefetch_Exc_hndlr	/* Abort - Prefetch Handler */
	b _abrtData_Exc_hndlr		/* Abort - Data Handler */
	b _reserved_Exc_hndlr		/* Reserved */
	b _IRQ_Exc_hndlr			/* IRQ Handler */
	b _FIQ_Exc_hndlr			/* FIQ Handler */

__start:	/* Real Entry to boot-code */

/* Setup Stacks for different modes */
/* Undefined mode stack */
	ldr r0, =_stack
	msr cpsr_c, #MODE_UND|I_F_BIT
	mov sp, r0
	sub r0, r0, #UND_STACK_SIZE

/* Abort mode stack */
	msr cpsr_c, #MODE_ABT|I_F_BIT
	mov sp, r0
	sub r0, r0, #ABT_STACK_SIZE

/* Fast interrupt stack */
	msr cpsr_c, #MODE_FIQ|I_F_BIT
	mov sp, r0
	sub r0, r0, #FIQ_STACK_SIZE

/* Interrupt Stack */
	msr cpsr_c, #MODE_IRQ|I_F_BIT
	mov sp, r0
	sub r0, r0, #IRQ_STACK_SIZE

/* SVC stack */
	msr cpsr_c, #MODE_SVC|I_F_BIT
	mov sp, r0
	sub r0, r0, #SVC_STACK_SIZE

/* User/System stack */
	msr cpsr_c, #MODE_SYS|I_F_BIT
	mov sp, r0

/* Invalidate and enable branch predictions */
	mov r0, #0
	mcr p15, #0, r0, c7, c5, #6
	isb
	mrc p15, #0, r0, c1, c0, #0
	orr r0, r0, #0x00000800
	mcr p15, #0, r0, c1, c0, #0

/* Enable Neon/VFP Co-Processor */
	mrc p15, #0, r1, c1, c0, #2
	orr r1, r1, #(0xf << 20)
	mcr p15, #0, r1, c1, c0, #2
	mov r1, #0
	mcr p15, #0, r1, c7, c5, #4
	mov r0, #0x40000000
	fmxr fpexc, r0

/* Clear BSS section */
	ldr r0, =_bss_start
	ldr r1, =(_bss_end - 0x04)
	mov r2, #0

clear_loop:
	str r2, [r0], #4
	cmp r0, r1
	ble clear_loop

/* Setup initial 1:1 virtual address
 * and switch on MMU
 */

.if 0
set_init_pgtbl:
	ldr r0, =tlb_l1_base						/* load TLB address to R0 */
	mcr p15, 0, r0, c2, c0, 0				/* Let the Co-processor know, where TLB can be found */

	ldr r1, =tlb_l1_base
	add r1, r1, #INIT_PGTBL_SIZE
	sub r1, r1, #4							/* Final address of TLB */

	movw r2, #SECTION_ENTRY_CTRL_BITS		/* Starting from 0x00000000|CTRL_BITS */

1:
	str r2, [r0], #0x4
	add r2, r2, #SECTION_NEXT
	cmp r0, r1
	ble 1b

/* Domain 0 */
	mov r0, #0x3
	mcr p15, 0, r0, c3, c0, 0

/* Enable MMU */
	mrc p15, 0, r0, c1, c0, 0
	orr r0, r0, #0x1
	mcr p15, 0, r0, c1, c0, 0
.endif

/* Enter kernel_init */
	ldr r10, =kernel_init
	mov lr, pc
	bx	r10

_hang:
	b _hang

.section .stack, "w"
.space INIT_STACK_SIZE


